package com.yuneec.droneservice.command;

import android.os.RemoteException;

import com.MAVLink.yuneec.msg_mav_rc_cmd;
import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.request.IRequest;
import com.yuneec.droneservice.request.IResponse;
import com.yuneec.droneservice.request.IResponseHandler;
import com.yuneec.droneservice.request.OnRequestListener;
import com.yuneec.mavlink.MavLinkProtocolPlugin;


/**
 * This is abstract class of command,provider default methods about command,command ID,command type.
 *
 * @author jw.gu
 * @version 1.0
 */
public abstract class BaseCommand extends IRequest {
    private msg_mav_rc_cmd cmd;

    protected BaseCommand(int id) {
        this(id, 254);
    }

    protected BaseCommand(int id, int size) {
        super(id);
        cmd = new msg_mav_rc_cmd(size);
        cmd.command = (short) id;
    }

    protected abstract short[] body();

    /**
     * Is there a reply after post command
     *
     * @return true if has a reply
     * @throws RemoteException
     */
    @Override
    public boolean hasReply() {
        return true;
    }

    protected boolean forceCompress() {
        return true;
    }

    public final byte[] toRawData() {
        if (forceCompress()) {
            short[] body = body();
            if (body != null && body.length > 0) {
                int len = body.length;
                while (body[len - 1] == 0) {
                    len--;
                    if (len == 0) {
                        break;
                    }
                }
                if (len == body.length) {
                    cmd.params = body;
                } else if (len > 0) {
                    cmd.params = new short[len];
                    System.arraycopy(body, 0, cmd.params, 0, len);
                }
            }
        } else {
            cmd.params = body();
        }
        return cmd.pack(0, 250).encodePacket(false);
    }

    @Override
    public boolean parseReply(IResponseHandler irh, OnRequestListener listener) {
        msg_mav_rc_cmd_ack handler = msg_mav_rc_cmd_ack.class.cast(irh);
        handler.initResponseHandler(toResponse(handler), listener);
        return true;
    }

    public abstract IResponse toResponse(msg_mav_rc_cmd_ack ack);

    @Override
    public int port() {
        return MavLinkProtocolPlugin.MAVLINK_MSG_ID;
    }

    public static long uIntArrayToLong(short[] data, int offset) {
        if (data == null || offset < 0 || data.length - offset < 8) {
            throw new IllegalArgumentException();
        }
        if (IS_BIGENDIAN) {
            return (0xffL & data[7])
                    | (0xff00L & (data[6] << 8))
                    | (0xff0000L & (data[5] << 16))
                    | (0xff000000L & (data[4] << 24))
                    | (0xff00000000L & (data[3] << 32))
                    | (0xff0000000000L & (data[2] << 40))
                    | (0xff000000000000L & (data[1] << 48))
                    | (0xff00000000000000L & (data[0] << 56));
        } else {
            return (0xffL & data[0])
                    | (0xff00L & (data[1] << 8))
                    | (0xff0000L & (data[2] << 16))
                    | (0xff000000L & (data[3] << 24))
                    | (0xff00000000L & (data[4] << 32))
                    | (0xff0000000000L & (data[5] << 40))
                    | (0xff000000000000L & (data[6] << 48))
                    | (0xff00000000000000L & (data[7] << 56));
        }
    }

    public static long uIntArrayToUInt(short[] data, int offset) {
        return uIntArrayToInt(data, offset) & 0xFFFFFFFFl;
    }

    public static int uIntArrayToInt(short[] data, int offset) {
        if (data == null || offset < 0) {
            throw new IllegalArgumentException();
        }
        if (data.length < offset + 4) {
            if (data.length == 2) {
                return uIntArrayToShort(data, offset);
            }
            return -1;
        }
        int iLowest;
        int iLow;
        int iMid;
        int iHigh;
        if (IS_BIGENDIAN) {
            iLowest = data[offset + 3];
            iLow = data[offset + 2];
            iMid = data[offset + 1];
            iHigh = data[offset + 0];
        } else {
            iLowest = data[offset + 0];
            iLow = data[offset + 1];
            iMid = data[offset + 2];
            iHigh = data[offset + 3];
        }
        return (iHigh << 24) | ((iMid & 0xFF) << 16) | ((iLow & 0xFF) << 8) | (0xFF & iLowest);
    }

    public static void intToUIntArray(short[] data, int off, int value) {
        if (data == null || off < 0 || data.length - off < 4) {
            throw new IllegalArgumentException();
        }
        short iLowest;
        short iLow;
        short iMid;
        short iHigh;

        iLowest = (short) (value & 0xFF);
        iLow = (short) ((value >> 8) & 0xFF);
        iMid = (short) ((value >> 16) & 0xFF);
        iHigh = (short) ((value >> 24) & 0xFF);

        if (IS_BIGENDIAN) {
            data[off + 3] = iLowest;
            data[off + 2] = iLow;
            data[off + 1] = iMid;
            data[off] = iHigh;
        } else {
            data[off] = iLowest;
            data[off + 1] = iLow;
            data[off + 2] = iMid;
            data[off + 3] = iHigh;
        }
    }

    public static short[] intToUIntArray(int value) {
        short[] array = new short[4];
        intToUIntArray(array, 0, value);
        return array;
    }


    public static Short uIntArrayToShort(short[] data, int offset) {
        if (data == null || offset < 0 || data.length - offset < 2) {
            throw new IllegalArgumentException();
        }
        int low;
        int high;
        if (IS_BIGENDIAN) {
            low = data[offset + 1];
            high = data[offset];
        } else {
            low = data[offset];
            high = data[offset + 1];
        }
        return (short) ((high << 8) | (0xFF & low));
    }

    public static int uIntArrayToUShort(short[] data, int offset) {
        short i = uIntArrayToShort(data, offset);
        if (i == -1) {
            return i;
        } else {
            return i & 0x0000FFFF;
        }
    }

    public static void shortToUIntArray(short[] data, int off, short value) {
        if (data == null || off < 0 || data.length - off < 2) {
            throw new IllegalArgumentException("array is null or len is less than 2!");
        }
        short iLow;
        short iHigh;
        iLow = (short) (value & 0xFF);
        iHigh = (short) ((value >> 8) & 0xFF);
        if (IS_BIGENDIAN) {
            data[off + 1] = iLow;
            data[off] = iHigh;
        } else {
            data[off] = iLow;
            data[off + 1] = iHigh;
        }
    }

    public static short[] shortToUIntArray(short value) {
        short[] array = new short[2];
        shortToUIntArray(array, 0, value);
        return array;
    }

    public static void byteArrayToUIntArray(short[] dst, int dstOff, byte[] src, int srcOff) {
        if (dst == null || dstOff < 0) {
            throw new IllegalArgumentException("dst array is illegal!");
        }
        if (src == null || srcOff < 0) {
            throw new IllegalArgumentException("src array is illegal!");
        }
        byteArrayToUIntArray(dst, dstOff, dst.length - dstOff, src, srcOff, src.length - srcOff);
    }

    public static void byteArrayToUIntArray(short[] dst, int dstOff, int dstLen, byte[] src, int srcOff, int len) {
        if (dst == null || dstOff < 0 || dstLen < 0 || dst.length - dstOff < dstLen) {
            throw new IllegalArgumentException("dst array is illegal!");
        }
        if (src == null || srcOff < 0 || len < 0 || src.length - srcOff < len) {
            throw new IllegalArgumentException("src array is illegal!");
        }
        if (dstLen < len) {
            throw new IllegalArgumentException("len of dst array is less than src array!");
        }
        for (int i = 0; i < len; i++) {
            dst[dstOff + i] = (short) (src[srcOff + i] & 0xFF);
        }
    }

    public static short[] byteArrayToUIntArray(byte[] src, int srcOff, int len) {
        if (src == null || srcOff < 0 || src.length - srcOff < len) {
            throw new IllegalArgumentException("src array is illegal!");
        }
        short[] dst = new short[len];
        byteArrayToUIntArray(dst, 0, len, src, srcOff, len);
        return dst;
    }

    public static void uIntArrayToByteArray(byte[] dst, int dstOff, int dstLen, short[] src, int srcOff, int len) {
        if (dst == null || dstOff < 0 || dstLen < 0 || dst.length - dstOff < dstLen) {
            throw new IllegalArgumentException("dst array is illegal!");
        }
        if (src == null || srcOff < 0 || len < 0 || src.length - srcOff < len) {
            throw new IllegalArgumentException("src array is illegal!");
        }
        if (dstLen < len) {
            throw new IllegalArgumentException("len of dst array is less than src array!");
        }
        for (int i = 0; i < len; i++) {
            dst[i] = (byte) (src[srcOff + i] & 0xFF);
        }
    }

    public static byte[] uIntArrayToByteArray(short[] src, int srcOff, int len) {
        if (src == null || srcOff < 0 || src.length - srcOff < len) {
            throw new IllegalArgumentException("src array is illegal!");
        }
        byte[] dst = new byte[len];
        uIntArrayToByteArray(dst, 0, len, src, srcOff, len);
        return dst;
    }

}
